#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
全局协调器和分布式黑板系统
精简版本，专注于核心协调功能
集成态势图系统用于ThreatScore计算
"""

import sys

sys.path.append('./py_trees-devel')
sys.path.append('./TaskAllocationCBBA')

import threading
import time
from typing import List, Dict, Tuple, Optional, Set
from dataclasses import dataclass, field
from enum import Enum
import math
import numpy as np
from world import World

# 导入agent.py中的Position类和Agent类
from agent import Position, Agent, Drone, Boat, AgentState, AGENT_PARAMS, TaskType

# 导入态势图相关模块
from target_probability_map import TargetProbabilityMap
from multi_target_prob_map import MultiTargetProbabilityMap


##############################################################################
# 基础数据结构
##############################################################################

class AgentType(Enum):
    WHITE_USV = "white_usv"
    WHITE_UAV = "white_uav"
    BLACK_USV = "black_usv"


# class AgentState(Enum):
#     FREE = "free"
#     FROZEN = "frozen"
#     CHARGING = "charging"
#     ELIMINATED = "eliminated"

class MissionType(Enum):
    PATROL = "patrol"
    INTERCEPT = "intercept"
    RECONNAISSANCE = "reconnaissance"
    SUPPORT = "support"
    DEFENSE = "defense"


class ThreatLevel(Enum):
    LOW = 1
    MEDIUM = 2
    HIGH = 3
    CRITICAL = 4


# Agent类直接从agent.py导入，不需要重复定义

# 为了兼容现有代码，创建Agent状态适配器
class AgentAdapter:
    """Agent适配器类，用于桥接agent.py中的Agent类和协调器需要的数据结构"""

    @staticmethod
    def get_agent_type(agent: Agent) -> AgentType:
        """获取智能体类型"""
        if isinstance(agent, Drone):
            return AgentType.WHITE_UAV
        elif isinstance(agent, Boat):
            if agent.faction == 'white':
                return AgentType.WHITE_USV
            else:
                return AgentType.BLACK_USV
        else:
            # 默认返回白方USV
            return AgentType.WHITE_USV

    @staticmethod
    def get_agent_state(agent: Agent) -> AgentState:
        """获取智能体状态"""
        return agent.state

    @staticmethod
    def get_position(agent: Agent) -> Position:
        """获取智能体位置"""
        # 如果是真实的Agent对象（有x, y属性）
        if hasattr(agent, 'x') and hasattr(agent, 'y'):
            return Position(agent.x, agent.y)
        # 如果是TestAgent对象（有position属性）
        elif hasattr(agent, 'position'):
            return agent.position
        else:
            # 默认位置
            print('Fail get_position!')
            return Position(0, 0)

    @staticmethod
    def get_battery_level(agent: Agent) -> float:
        """获取电池电量"""
        if hasattr(agent, 'power') and hasattr(agent, 'max_power'):
            return (agent.power / agent.max_power) * 100.0
        else:
            return 100.0

    @staticmethod
    def get_capabilities(agent: Agent) -> Set[str]:
        """获取智能体能力"""
        capabilities = set()
        if isinstance(agent, Drone):
            capabilities.update({'reconnaissance', 'surveillance'})
        elif isinstance(agent, Boat):
            capabilities.update({'combat', 'surveillance'})
            if agent.faction == 'white':
                capabilities.add('defense')
            else:
                capabilities.add('penetration')
        return capabilities

    @staticmethod
    def get_sensor_range(agent: Agent) -> float:
        """获取传感器范围"""
        return agent.sensor_range

    @staticmethod
    def get_sensor_angle(agent: Agent) -> float:
        """获取传感器角度"""
        return agent.sensor_angle


# @dataclass
# class ThreatContact:
#     contact_id: int
#     position: Position
#     last_seen: float
#     velocity: List[float] = [0.0, 0.0]
#     estimated_destination: Optional[Position] = None
#     enemy_scan_range: float = AGENT_PARAMS['BSV_SCAN_RANGE']
#     enemy_lock_range: float = AGENT_PARAMS['BSV_LOCK_RANGE']


@dataclass
class GlobalMission:
    mission_id: str
    mission_type: MissionType
    priority: int
    target_area: Dict
    required_capabilities: Set[str]
    assigned_agents: List[str] = field(default_factory=list)
    status: str = "pending"
    deadline: Optional[float] = None


##############################################################################
# 分布式黑板系统
##############################################################################

class DistributedBlackboard:
    """分布式黑板，用于智能体间信息共享和全局协调"""

    def __init__(self, world):
        self._lock = threading.Lock()
        self.agents: Dict[str, Agent] = {}
        self.enemy_contacts: Dict[str, Tuple[Position, float]] = {}  # 敌情：位置+时间戳
        self.coordination_requests: List[Dict] = []

        # 全局协调相关数据
        self.coordination_data: Optional[Dict] = None
        self.mission_assignments: Dict[str, str] = {}  # agent_id -> mission_id

        self.world = world

        # 集成态势图系统
        self.situation_map = world.faction_maps['white']
        # 白方估计黑方掌握的白方态势图
        self.enemy_situation_maps = MultiTargetProbabilityMap(world.width, world.height, world.grid_resolution)

        for agent in self.agents.values():
            if isinstance(agent, Boat):
                self.enemy_situation_maps.add_target(agent.id, [agent.x, agent.y])

        # # 态势图更新计数器
        # self.situation_update_counter = 0
        # self.situation_update_interval = 5  # 每5个更新周期更新一次态势图

    def update_agent_status(self, agent: Agent):
        with self._lock:
            self.agents[str(agent.id)] = agent

    def tick_enemy_situation_maps(self, agent: Agent):
        """
        更新黑方对白方的态势图，让白方USV的位置概率扩散
        """
        with self._lock:
            self.enemy_situation_maps.predict()

    def update_enemy_situation_maps(self, agent: Agent):
        """
        白方USV被锁定时，更新黑方对白方的态势图
        """
        with self._lock:
            self.enemy_situation_maps.add_target(agent.id, [agent.x, agent.y])

    def report_enemy_contact(self, contact_id: str, position: Position, timestamp: float):
        with self._lock:
            self.enemy_contacts[contact_id] = (position, timestamp)
            self.situation_map.add_target(contact_id, position)

    def get_nearby_enemies(self, position: Position, radius: float) -> List[Tuple[str, Position]]:
        with self._lock:
            nearby = []
            for contact_id, (enemy_pos, timestamp) in self.enemy_contacts.items():
                if position.distance_to(enemy_pos) <= radius:
                    nearby.append((contact_id, enemy_pos))
            return nearby

    def get_mission_assignment(self, agent_id: str) -> Optional[str]:
        """获取智能体的任务分配"""
        with self._lock:
            return self.mission_assignments.get(agent_id)

    def update_mission_assignment(self, agent_id: str, mission_id: str):
        """更新智能体的任务分配"""
        with self._lock:
            self.mission_assignments[agent_id] = mission_id

    def get_coordination_data(self) -> Optional[Dict]:
        """获取最新的协调数据"""
        with self._lock:
            return self.coordination_data

    def get_all_agents(self) -> Dict[str, Agent]:
        """获取所有智能体"""
        with self._lock:
            return self.agents

    def get_situation_map(self) -> MultiTargetProbabilityMap:
        """获取态势图"""
        with self._lock:
            return self.situation_map

    def get_threat_probability_at(self, position: Position) -> float:
        """获取指定位置的威胁概率"""
        with self._lock:
            try:
                # 将实际坐标转换为网格坐标
                grid_x = int(position.x / self.situation_map.grid_resolution)
                grid_y = int(position.y / self.situation_map.grid_resolution)

                # 确保坐标在有效范围内
                if (0 <= grid_x < self.situation_map.grid_width and
                        0 <= grid_y < self.situation_map.grid_height):

                    # 合并所有目标的概率
                    total_prob = 0.0
                    for target_map in self.situation_map.target_maps.values():
                        total_prob += target_map.prob_map[grid_y, grid_x] * target_map.belief

                    return min(1.0, total_prob)

                return 0.0
            except Exception as e:
                print(f"获取威胁概率失败: {e}")
                return 0.0

    def get_prob_nearby_at(self, path_points: List[Position], enemy_situation_maps: MultiTargetProbabilityMap,
                           radius: float = 40000) -> float:
        """
        获取指定位置附近的威胁概率
        enemy_situation_maps: 白方估计黑方对白方的态势图
        """

        with self._lock:
            try:
                map_probs = []

                for enemy_situation_map in enemy_situation_maps.target_maps.values():

                    prob_map_sum = 0.0
                    for position in path_points:
                        # 将实际坐标转换为网格坐标
                        grid_x = int(position.x / self.situation_map.grid_resolution)
                        grid_y = int(position.y / self.situation_map.grid_resolution)

                        # 计算采样半径内的威胁概率
                        grid_radius = int(radius / self.situation_map.grid_resolution)
                        for dx in range(-grid_radius, grid_radius + 1):
                            for dy in range(-grid_radius, grid_radius + 1):

                                nx, ny = grid_x + dx, grid_y + dy

                                if (0 <= nx < self.situation_map.grid_width and
                                        0 <= ny < self.situation_map.grid_height):
                                    prob = enemy_situation_map.prob_map[ny, nx] * enemy_situation_map.belief
                                    prob_map_sum += prob

                    map_probs.append(1 - prob_map_sum)

                return np.prod(map_probs)

            except Exception as e:
                print(f"获取指定位置附近的威胁概率失败: {e}")
                return 0.0

    def get_information_entropy_at(self, position: Position, radius: float = 40000) -> float:

        """获取指定位置的信息熵"""
        with self._lock:
            try:
                # 将实际坐标转换为网格坐标
                grid_x = int(position.x / self.situation_map.grid_resolution)
                grid_y = int(position.y / self.situation_map.grid_resolution)

                # 计算采样半径内的信息熵
                grid_radius = int(radius / self.situation_map.grid_resolution)

                entropy_sum = 0.0
                valid_points = 0

                for dx in range(-grid_radius, grid_radius + 1):
                    for dy in range(-grid_radius, grid_radius + 1):
                        nx, ny = grid_x + dx, grid_y + dy

                        if (0 <= nx < self.situation_map.grid_width and
                                0 <= ny < self.situation_map.grid_height):

                            # 计算该点的信息熵
                            point_entropy = 0.0

                            for target_map in self.situation_map.target_maps.values():
                                prob = target_map.prob_map[ny, nx] * target_map.belief
                                if prob > 0:
                                    point_entropy += -prob * np.log2(prob + 1e-6)

                            entropy_sum += point_entropy
                            valid_points += 1

                return entropy_sum / max(valid_points, 1)

            except Exception as e:
                print(f"获取信息熵失败: {e}")
                return 0.0


##############################################################################
# 全局协调控制器
##############################################################################

class GlobalCoordinator:
    """全局协调控制器 - 整合所有协调层组件"""

    def __init__(self, max_step: int = 300, world_width: int = 100000, world_height: int = 100000,
                 end_line_x: int = 400, world: World = None):
        # 创建分布式黑板（包含态势图）
        # self.blackboard = DistributedBlackboard(world_width, world_height)
        if hasattr(Agent, '_shared_blackboard') and Agent._shared_blackboard is not None:
            self.blackboard = Agent._shared_blackboard
        else:
            Agent._shared_blackboard = DistributedBlackboard(world)
            self.blackboard = Agent._shared_blackboard

        self.world = self.blackboard.world

        # 三大核心组件实例
        self.enhanced_task_allocator = EnhancedTaskAllocationCoordinator(max_step, self.blackboard, end_line_x,
                                                                         self.world)
        # self.situation_fusion = SituationAwarenessFusion()
        # self.strategy_maker = StrategyDecisionMaker()

        # 协调控制参数
        self.usv_lock_reallocation_interval = 150.0  # USV: Lock任务重分配：150秒协调周期
        self.uav_charge_reallocation_interval = 400.0  # UAV: 充电任务重分配：10min协调周期
        self.uav_update_map_reallocation_interval = 400.0  # UAV: 更新态势图任务重分配：10min协调周期
        self.coordination_cycle_interval = 1.0  # GC全局协调更新：1.0秒协调周期
        self.is_running = False
        self.coordination_thread = None
        self.clusters_K = 3  # k-means算法需要分类的簇的数量

        # 事件触发参数
        self.event_triggers = []
        self.last_usv_lock_allocation_time = 0.0
        self.last_uav_charge_allocation_time = 0.0
        self.last_uav_update_map_allocation_time = 0.0
        self.use_enhanced_allocation = True  # 是否使用增强版分配器
        self.all_explore_done = False

        # 执行Locking任务状态
        self.status_locking_exe = False
        self.status_reckon_exe = False
        # 判断各白方USV是否满足呼叫UAV协助探测条件
        self.status_call_uav = np.zeros(len(self.blackboard.world.faction_agents['white']), dtype=bool)

    def start_coordination(self, external_blackboard: DistributedBlackboard = None):
        """启动协调循环"""
        # 使用外部黑板或内部黑板
        blackboard = external_blackboard if external_blackboard else self.blackboard

        # 如果使用外部黑板，更新任务分配器的黑板引用
        if external_blackboard:
            self.enhanced_task_allocator.set_blackboard(external_blackboard)

        self.is_running = True
        # self.coordination_thread = threading.Thread(
        #     target=self._coordination_loop, 
        #     args=(blackboard,)
        # )
        # self.coordination_thread.start()

    def stop_coordination(self):
        """停止协调循环"""
        self.is_running = False
        if self.coordination_thread:
            self.coordination_thread.join()

    def coordination_loop(self):
        """主协调循环"""
        try:
            current_time = self.world.time

            # # 检查是否需要事件触发的重分配
            # event_triggered = self._check_event_triggers(self.blackboard)

            # 协同Lock任务分配 TODO:修改事件触发条件
            if (current_time - self.last_usv_lock_allocation_time) >= self.usv_lock_reallocation_interval:
                if self.use_enhanced_allocation:
                    try:
                        # 使用增强版CBBA分配器
                        # self.enhanced_task_allocator.execute_lock_operation()
                        clusters = self.enhanced_task_allocator.cluster_threats_in_K(self.clusters_K, self.world.faction_maps['white'].target_maps)
                        result = self.enhanced_task_allocator.allocate_usv_to_clusters(clusters)
                        # 按簇分别执行 Lock 分配
                        all_threats = self.world.faction_maps['white'].target_maps
                        # 筛选 belief > 0 即存活的威胁目标
                        filtered_threats = {tid: threat for tid, threat in all_threats.items() if threat.belief > 0}
                        for cid, threat_ids in clusters.items():
                            usv_list = result.get(cid, [])
                            # 从筛选后的威胁目标中构建 cluster_threats
                            cluster_threats = {tid: filtered_threats[tid] for tid in threat_ids if tid in filtered_threats}
                            if cluster_threats and usv_list:
                                self.enhanced_task_allocator.execute_lock_operation(cluster_threats, usv_list)
                        print("cluster任务分配完成")
                    except Exception as e:
                        import traceback
                        print(traceback.format_exc())
                        print(f"Lock任务分配失败: {e}")
                self.last_usv_lock_allocation_time = self.world.time

            # 协同锁定/探测任务分配
            drones_candidates = []
            for agent in self.world.faction_agents['white']:
                if isinstance(agent, Drone):
                    if (agent.state is AgentState.FREE or agent.state is AgentState.STANDBY) and agent.task_type != TaskType.RTB:
                        drones_candidates.append(agent)
                        if agent.task_type == TaskType.LOCK:
                            agent.task_list.clear()
                            agent.task_type = TaskType.IDLE

            for agent in self.world.faction_agents['white']:
                if isinstance(agent, Boat):
                    if agent.task_type == TaskType.LOCK and agent.task_list:
                        target_id = agent.task_list[0]['task_id']
                        target = self.world.faction_maps['white'].target_maps[target_id]
                        target_position = target.position
                        drones_available = []
                        for drone in drones_candidates:
                            # 仅选取航程可能满足任务需求的无人机
                            if drone.distance_to(target_position) < (drone.power - 300) * drone.max_speed * 0.4:
                                drones_available.append(drone)
                        distance_threshold = self.enhanced_task_allocator.calculate_distance_threshold(target, agent,
                                                                                                       drones_available)
                        try:
                            # 计算到目标的距离
                            distance_to_target = agent.distance_to(target_position)
                            if distance_to_target <= distance_threshold:  # 满足呼叫UAV协助探测条件
                                self.enhanced_task_allocator.call_uav_assit_scan(agent, drones_available)
                        except Exception as e:
                            import traceback
                            print(f"无人机协助探测任务分配失败: {e}")
                            print(traceback.format_exc())

            # UAV的charge任务分配
            if (current_time - self.last_uav_charge_allocation_time) >= self.uav_charge_reallocation_interval:
                self.enhanced_task_allocator.execute_charge_update()
                print(f"UAV: {self.world.time}: 充电任务分配完成")
                self.last_uav_charge_allocation_time = self.world.time

            # UAV的更新态势图任务（Reckon：Update_Map)任务分配
            if (current_time - self.last_uav_update_map_allocation_time) >= self.uav_update_map_reallocation_interval:
                self.enhanced_task_allocator.execute_reckon_update()
                print(f"UAV: {self.world.time}: 更新态势图任务分配完成")
                self.last_uav_update_map_allocation_time = self.world.time



        except Exception as e:
            import traceback
            print(traceback.format_exc())
            print(f"协调循环错误: {e}")
            time.sleep(1)  # 错误时短暂等待


# 增强版任务分配协调器 - 集成CBBA算法
##############################################################################

@dataclass
class EnhancedTask:
    """增强版任务定义"""
    task_id: int
    target_id: int
    task_type: TaskType
    position: Position
    velocity: List[float]
    time_window: Tuple[float, float]  # (start_time, end_time)
    duration: float
    value: float
    threat_score: float = 0.0
    priority: int = 0
    required_capabilities: Set[str] = field(default_factory=set)

    # # 任务特定参数
    enemy_scan_range: float = AGENT_PARAMS['BSV_SCAN_RANGE']
    enemy_lock_range: float = AGENT_PARAMS['BSV_LOCK_RANGE']
    # explore_area_radius: float = 0.0  # 探索区域半径
    # threat_contact_id: int = 0        # 威胁联系人ID
    # intercept_position: Position = None  # 拦截位置


class CBBAIntegrator:
    """CBBA算法集成器"""

    def __init__(self, max_step: int = 300, blackboard: DistributedBlackboard = None):
        # 导入CBBA相关模块
        from TaskAllocationCBBA.CBBA_Scan import CBBA_Scan
        from TaskAllocationCBBA.CBBA_Lock import CBBA_Lock
        from TaskAllocationCBBA.CBBAAgent import CBBAAgent
        from TaskAllocationCBBA.CBBATask import CBBATask
        from TaskAllocationCBBA.WorldInfo import WorldInfo
        from TaskAllocationCBBA.DistributedAuction import distributed_auction_with_virtual_usv

        self.CBBA_Scan = CBBA_Scan
        self.CBBA_Lock = CBBA_Lock
        self.CBBAAgent = CBBAAgent
        self.CBBATask = CBBATask
        self.distributed_auction_with_virtual_usv = distributed_auction_with_virtual_usv
        self.WorldInfo = WorldInfo

        # 添加黑板引用
        self.blackboard = blackboard

        # 智能体类型映射
        self.agent_type_mapping = {
            AgentType.WHITE_USV: 0,
            AgentType.WHITE_UAV: 1,
            AgentType.BLACK_USV: 2
        }

        # 任务类型映射
        self.task_type_mapping = {
            TaskType.IDLE: 0,
            TaskType.EXPLORE: 1,
            TaskType.RTB: 2,
            TaskType.CHARGE: 3,
            TaskType.RECKON: 4,
            TaskType.ASSIT_LOCK: 5,
            TaskType.MOVETOPOSITION: 6,
            TaskType.LOCK: 7
        }

        # 配置数据 - 使用传入的max_step作为DURATION
        self.config_data = {
            "AGENT_TYPES": ["usv", "uav", "bsv"],
            "TASK_TYPES": ["idle", "explore", "rtb", "charge", "reckon", "assit_lock", "move_to_position", "lock"],
            "TASK_DEFAULT": {"START_TIME": 0, "END_TIME": 36000, "DURATION": max_step},
        }

        self.cbba_scan_solver = self.CBBA_Scan(self.config_data)
        self.cbba_lock_solver = self.CBBA_Lock(self.config_data, self.blackboard.world)

    def convert_agents_to_cbba_format(self, agents: List[Agent]) -> List:
        """转换智能体为CBBA格式"""
        cbba_agents = []

        for i, agent in enumerate(agents):
            # 使用适配器获取智能体信息
            agent_type = AgentAdapter.get_agent_type(agent)
            position = AgentAdapter.get_position(agent)
            battery_level = AgentAdapter.get_battery_level(agent)

            cbba_agent = self.CBBAAgent(
                agent_id=i,
                Agent_id=agent.id,
                agent_type=self.agent_type_mapping.get(agent_type, 0),
                availability=0.0,
                remainRange=20000.0 if agent_type == AgentType.WHITE_USV else 60000.0,
                remainEnergy=battery_level,
                x=position.x,
                y=position.y,
                z=0.0,
                nom_velocity=agent.max_speed,
                fuel=0.1,
                max_depth=agent.max_depth
            )
            cbba_agents.append(cbba_agent)

        return cbba_agents

    def convert_tasks_to_cbba_format(self, tasks: List[EnhancedTask]) -> List:
        """转换任务为CBBA格式"""
        cbba_tasks = []

        for task in tasks:
            cbba_task = self.CBBATask(
                task_id=task.task_id,
                target_id=task.target_id,
                task_type=self.task_type_mapping.get(task.task_type, 0),
                task_value=task.value,
                start_time=task.time_window[0],
                end_time=task.time_window[1],
                duration=task.duration,
                discount=0.1,
                x=task.position.x,
                y=task.position.y,
                z=0.0,
                velocity=task.velocity,
                enemy_scan_range=task.enemy_scan_range,
                enemy_lock_range=task.enemy_lock_range
            )
            cbba_tasks.append(cbba_task)

        return cbba_tasks

    def solve_charge_task_allocation_with_distributed_auction(self, drones: List[Drone], boats: List[Boat]):
        """使用分布式拍卖算法求解UAV的Charge任务分配"""

        # 创建拍卖价格t_avail矩阵
        M_tavail = self.create_tavail_matrix(drones, boats)

        # 转换结果 - 修正索引映射
        final_allocation_result = {}

        # 使用分布式拍卖算法求解 TODO 兼容虚拟节点情况，无人机数量少于船时出现问题，无人机全部停靠时不收敛
        allocation_result, _ = self.distributed_auction_with_virtual_usv(M_tavail)

        drone_ids = [drone.id for drone in drones]
        boats_ids = [boat.id for boat in boats]

        for i, drone_id in enumerate(drone_ids):
            if allocation_result[i] == 114514:
                # 兼容虚拟节点情况
                drones[drone_id].rtb_target = None
                continue

            if i < len(allocation_result) and allocation_result[i]:
                final_allocation_result[drone_id] = boats_ids[allocation_result[i] - 1]  # allocation_result使用的是1-index
                drones[i].rtb_target = boats[allocation_result[i] - 1]
                drones[i].t_avail = -M_tavail[allocation_result[i] - 1][i]

        return final_allocation_result

    def create_tavail_matrix(self, drones: List[Drone], boats: List[Boat]):
        """创建拍卖价格矩阵"""
        H = np.zeros((len(boats), len(drones)))
        M = -10000
        v_drone = drones[0].max_speed

        for i, boat in enumerate(boats):
            for j, drone in enumerate(drones):
                t_travel = boat.distance_to(drone) / v_drone
                if boat.drone is None:
                    H[i, j] = t_travel
                else:
                    drone_charge = boat.drone
                    t_charge = (drone_charge.max_power - drone_charge.power) * 7200 / 100
                    H[i, j] = max(t_travel, t_charge)
        return H

    def solve_scan_task_allocation(self, agents: List[Agent], tasks: List[EnhancedTask]) -> Dict[int, List[int]]:
        """使用CBBA算法求解任务分配"""
        if not tasks:
            # 如果没有任务，返回空分配
            return {agent.id: [] for agent in agents}

        # 转换数据格式
        cbba_agents = self.convert_agents_to_cbba_format(agents)
        cbba_tasks = self.convert_tasks_to_cbba_format(tasks)

        # 智能体最多执行任务数量
        max_depth = cbba_agents[0].max_depth

        # 创建世界信息
        world_info = self.WorldInfo(
            limit_x=[0, self.blackboard.world.width],
            limit_y=[0, self.blackboard.world.height],
            limit_z=[0, 0]
        )

        # 求解
        try:
            path_list, times_list = self.cbba_scan_solver.solve(
                cbba_agents, cbba_tasks, world_info,
                max_depth, time_window_flag=True
            )

            # 转换结果 - 修正索引映射
            allocation_result = {}
            agent_ids = [agent.id for agent in agents]

            for i, agent_id in enumerate(agent_ids):
                if i < len(path_list) and path_list[i]:
                    # 根据实际任务ID映射结果
                    task_ids = []
                    for task_idx in path_list[i]:
                        task_ids.append(task_idx)
                    allocation_result[agent_id] = task_ids
                else:
                    allocation_result[agent_id] = []

            return allocation_result

        except Exception as e:
            print(f"CBBA求解错误: {e}")
            # 返回空分配而不是空字典
            return {agent.id: [] for agent in agents}

    def solve_lock_task_allocation(self, agents: List[Agent], tasks: List[EnhancedTask]) -> Dict[int, List[int]]:
        """使用CBBA算法求解任务分配"""
        if not tasks:
            # 如果没有任务，返回空分配
            return {agent.id: [] for agent in agents}, None, None

        # 转换数据格式
        cbba_agents = self.convert_agents_to_cbba_format(agents)
        cbba_tasks = self.convert_tasks_to_cbba_format(tasks)

        # 智能体最多执行任务数量
        max_depth = cbba_agents[0].max_depth

        # 创建世界信息
        world_info = self.WorldInfo(
            limit_x=[0, self.blackboard.world.width],
            limit_y=[0, self.blackboard.world.height],
            limit_z=[0, 0]
        )

        # 求解
        try:
            path_list, times_list = self.cbba_lock_solver.solve(
                cbba_agents, cbba_tasks, world_info,
                max_depth, time_window_flag=True
            )

            # 转换结果 - 修正索引映射
            allocation_result = {}
            agent_ids = [agent.id for agent in agents]

            for i, agent_id in enumerate(agent_ids):
                if i < len(path_list) and path_list[i]:
                    # 根据实际任务ID映射结果
                    task_ids = []
                    for task_idx in path_list[i]:
                        if task_idx < len(tasks) and task_idx > -1:
                            task_ids.append(tasks[task_idx].target_id)
                    allocation_result[agent_id] = task_ids
                else:
                    allocation_result[agent_id] = []

            return allocation_result

        except Exception as e:
            import traceback
            print(traceback.format_exc())
            print(f"CBBA求解错误: {e}")
            # 返回空分配而不是空字典
            return {}, None, None


class EnhancedTaskAllocationCoordinator:
    """增强版任务分配协调器 - 集成CBBA算法和三种任务类型"""

    def __init__(self, max_step: int = 300, blackboard: DistributedBlackboard = None, end_line_x: int = 400,
                 world: World = None):
        self.active_tasks: Dict[str, EnhancedTask] = {}
        self.blackboard = blackboard  # 引用黑板以获取态势图数据
        self.world = world

        # 任务生成参数
        self.explore_grid_size = blackboard.situation_map.grid_resolution  # 探索网格大小(m)
        self.threat_scan_radius = 20000  # 威胁扫描半径(m)  
        self.intercept_prediction_time = 30.0  # 拦截预测时间(s)
        self.end_line_x = end_line_x  # 防御线x坐标

        # 初始化CBBA求解器
        self.cbba_integrator = CBBAIntegrator(max_step, blackboard)

        # 权重参数
        self.weight_params = {
            'w_T': 1.0,  # ThreatScore权重
            'w_D': 1.2,  # TravelTime权重
            # 'w_P': 2.0,    # 突防概率权重

            # 计算ThreatScore的参数
            'alpha': 1.0,  # 突防概率权重
            'beta': 0.8,  # 距离衰减因子
            'gamma': 0.5,  # 航向一致性权重
            'delta': 0.1,  # 速度权重
            'epsilon': 0.2  # 信息熵权重
        }

        # 突防概率计算方法
        self.use_simple_penetration = False
        self.use_grid_penetration = False
        self.use_situational_map = True  # 新增：使用态势图计算

    def set_blackboard(self, blackboard: DistributedBlackboard):
        """设置黑板引用"""
        self.blackboard = blackboard

    def generate_threat_scan_tasks(self, threats: Dict[int, TargetProbabilityMap], agents: List[Agent]) -> List[
        EnhancedTask]:
        """生成威胁扫描任务(Scan_Faction)"""
        tasks = []
        current_time = self.world.time

        for i, threat in threats.items():
            # 计算威胁评分
            threat_score = self._calculate_threat_score(threat)

            # 计算agents中各agent到thread的时间，并进行平均归一化，用以表示threat与agents的远近
            travel_times = self._calculate_travel_time(threat, agents)
            travel_times_mean = sum(travel_times) / len(travel_times)
            normalized_mean_travel_time = travel_times_mean / max(travel_times)

            task_value = self.weight_params['w_T'] * threat_score - self.weight_params[
                'w_D'] * normalized_mean_travel_time

            task = EnhancedTask(
                task_id=i,
                target_id=threat.target_id,
                task_type=TaskType.RECKON,
                position=Position(threat.position[0], threat.position[1]),
                priority=3,
                time_window=(current_time - 5, current_time + 3600/2),  # 60分钟窗口
                duration=travel_times_mean,
                value=task_value * 20,
                threat_score=threat_score,
                velocity=threat.velocity,
                required_capabilities={'surveillance'}
            )
            tasks.append(task)

        return tasks

    def generate_intercept_tasks(self, threats: Dict[int, TargetProbabilityMap], agents: List[Agent]) -> List[EnhancedTask]:
        """
        生成白方USV针对黑方USV的拦截任务
        当威胁数量小于可用智能体数量时，复制高价值任务以充分利用空闲智能体
        """

        tasks = []
        current_time = self.world.time

        # 如果没有威胁，直接返回空任务列表
        if not threats:
            return tasks

        # 为每个威胁生成一个基础任务
        threat_tasks = []
        for i, threat in enumerate(threats.values()):
            # 计算拦截价值
            threat_score = self._calculate_threat_score(threat)
            travel_times = self._calculate_travel_time(threat, agents)
            travel_times_mean = sum(travel_times) / len(travel_times)
            normalized_mean_travel_time = travel_times_mean / max(travel_times)
            intercept_value = self.weight_params['w_T'] * threat_score - self.weight_params[
                'w_D'] * normalized_mean_travel_time

            task = EnhancedTask(
                task_id=i,
                target_id=threat.target_id,
                task_type=TaskType.LOCK,
                position=Position(threat.position[0], threat.position[1]),
                velocity=threat.velocity,
                priority=4,
                time_window=(current_time - 5, current_time + 114514),  # 120分钟窗口
                duration=300,  # 5分钟锁定时间
                value=intercept_value,
                threat_score=threat_score,
                enemy_scan_range=AGENT_PARAMS['BSV_SCAN_RANGE'],
                enemy_lock_range=AGENT_PARAMS['BSV_LOCK_RANGE'],
            )
            threat_tasks.append(task)

        # 添加基础任务到任务列表
        tasks.extend(threat_tasks)

        # 如果威胁数量少于可用智能体数量，则复制高价值任务
        num_agents = len(agents)
        num_threats = len(threat_tasks)

        if num_agents > num_threats > 0:
            # 按价值排序任务，价值高的在前
            threat_tasks.sort(key=lambda x: x.value, reverse=True)

            # 计算需要复制的任务数量
            tasks_needed = num_agents - num_threats

            # 循环复制高价值任务，直到满足智能体数量需求
            for i in range(tasks_needed):
                # 选择要复制的任务（按价值顺序循环选择）
                task_to_copy = threat_tasks[i % num_threats]

                # 创建复制的任务，保持唯一性
                copied_task = EnhancedTask(
                    task_id=len(threat_tasks) + i,  # 确保任务ID唯一
                    target_id=task_to_copy.target_id,
                    task_type=task_to_copy.task_type,
                    position=task_to_copy.position,
                    velocity=task_to_copy.velocity,
                    priority=task_to_copy.priority,
                    time_window=task_to_copy.time_window,
                    duration=task_to_copy.duration,
                    value=task_to_copy.value,  # 保持相同价值
                    threat_score=task_to_copy.threat_score,
                    enemy_scan_range=task_to_copy.enemy_scan_range,
                    enemy_lock_range=task_to_copy.enemy_lock_range,
                )
                tasks.append(copied_task)

        return tasks


    def _calculate_information_entropy(self, position: Position, threat_grid: List) -> float:
        """
        计算信息熵 - 优先使用态势图数据
        用于探索任务价值评估
        """
        # 优先使用态势图的信息熵
        if self.blackboard:
            try:
                return self.blackboard.get_information_entropy_at(position, radius=self.explore_grid_size)
            except Exception as e:
                print(f"获取态势图信息熵失败: {e}")

        # 备用方法：基于威胁网格计算
        if not threat_grid:
            return 5.0  # 默认中等信息熵

        # 基于周围网格的不确定性计算信息熵
        grid_size = len(threat_grid)
        if grid_size == 0:
            return 5.0

        # 将位置映射到网格索引
        grid_x = int(position.x / 100) % grid_size
        grid_y = int(position.y / 100) % grid_size

        # 计算周围3x3区域的平均不确定性
        entropy_sum = 0.0
        count = 0

        for dx in [-1, 0, 1]:
            for dy in [-1, 0, 1]:
                nx, ny = grid_x + dx, grid_y + dy
                if 0 <= nx < grid_size and 0 <= ny < grid_size:
                    # 信息熵基于概率的不确定性
                    prob = threat_grid[nx][ny]
                    if prob > 0:
                        entropy_sum += -prob * math.log(prob + 1e-6)
                    count += 1

        return entropy_sum / max(count, 1)

    def _calculate_distance_to_defense_line(self, position: Position) -> float:
        """计算到防线的距离"""
        return abs(position.x - self.end_line_x)

    def _calculate_threat_score(self, threat: TargetProbabilityMap) -> float:
        """
        计算威胁评分，按照设计文档的公式：
        ThreatScore = α·P_pen + β·D_factor + γ·max(0,Θ_heading) + δ·V_factor
        """
        # 1. 计算突防概率 P_pen 以及最小突防距离 min_exit_distance
        pen_prob, min_exit_distance = self._calculate_penetration_probability_v2(threat)

        # 2. 计算距离衰减因子 D_factor = 1/(1 + distance/1000)
        distance_factor = 1.0 / (1.0 + min_exit_distance / 1000)

        # 3. 计算航向一致性 Θ_heading = cos(angle(heading, direction_to_exit))
        heading_consistency = self._calculate_heading_consistency(threat)

        # 4. 计算速度权重 V_factor = velocity/v_max
        velocity_magnitude = (threat.velocity[0] ** 2 + threat.velocity[1] ** 2) ** 0.5
        v_max = 10.0  # BSV最大速度10 m/s
        velocity_factor = min(1.0, velocity_magnitude / v_max)

        # 5. 计算目标对应的信息熵，信息熵越低则潜在威胁越大
        entropy = threat.get_entropy()

        # 5. 综合威胁评分
        threat_score = (self.weight_params['alpha'] * pen_prob +
                        self.weight_params['beta'] * distance_factor +
                        self.weight_params['gamma'] * max(0, heading_consistency) +
                        self.weight_params['delta'] * velocity_factor +
                        self.weight_params['epsilon'] * (1 - entropy))

        return max(0, min(10, threat_score))

    def _calculate_travel_time(self, threat: TargetProbabilityMap, agents: List[Agent]) -> List[float]:
        """
        计算agents中每个agent到threat的需要时间
        """
        travel_times = []

        for agent in agents:
            try:
                # 获取智能体位置
                agent_position = AgentAdapter.get_position(agent)

                # 计算到威胁位置的距离
                threat_pos = Position(threat.position[0], threat.position[1])
                distance = agent_position.distance_to(threat_pos)

                # 获取智能体状态
                agent_state = AgentAdapter.get_agent_state(agent)

                # 根据智能体类型获取最大速度
                if isinstance(agent, Drone):
                    max_speed = agent.max_speed  # UAV最大速度150 m/s

                    # 计算基础旅行时间
                    if max_speed > 0:
                        base_travel_time = distance / max_speed
                    else:
                        base_travel_time = float('inf')

                    if agent_state == AgentState.CHARGING:
                        # 充电状态需要额外时间（假设还需要充电时间）
                        if hasattr(agent, 'power') and hasattr(agent, 'max_power'):
                            battery_ratio = agent.power / agent.max_power
                            charging_time = 0.5 * (1.0 - battery_ratio) * 5 * 3600  # 充电时间是续航时间的5倍，保守估计充电时间：乘以 0.5
                            travel_time = base_travel_time + charging_time
                    else:
                        travel_time = base_travel_time


                elif isinstance(agent, Boat):
                    max_speed = agent.max_speed  # USV最大速度10 m/s

                    # 计算基础旅行时间
                    if max_speed > 0:
                        base_travel_time = max(0, distance - 35000) / max_speed  # 进入锁定范围：减去35km的距离
                    else:
                        base_travel_time = float('inf')

                    # 根据智能体状态调整时间
                    if agent_state == AgentState.FROZEN:
                        # 冻结状态需要额外300秒恢复时间
                        travel_time = base_travel_time + (agent.freeze_recover_time - agent.freeze_recover_timer)
                    elif agent_state == AgentState.ELIMINATED:
                        # 已被淘汰的智能体无法到达
                        travel_time = float('inf')
                    else:
                        # 正常状态
                        travel_time = base_travel_time

                travel_times.append(travel_time)

            except Exception as e:
                print(f"计算智能体 {agent.id} 到威胁 {threat.target_id} 的旅行时间失败: {e}")
                import traceback
                print(traceback.format_exc())
                travel_times.append(float('inf'))

        return travel_times

    def _calculate_penetration_probability_v2(self, threat: TargetProbabilityMap) -> Tuple[float, float]:
        """
        改进的突防概率计算 - 基于实时探测覆盖和态势图
        提供更简洁可靠的替代方案
        """
        # 方法1：基于态势图的计算（推荐）
        if hasattr(self, 'use_situational_map') and self.use_situational_map and self.blackboard:
            return self._calculate_penetration_situational_map(threat)
        else:
            print("利用态势图计算突防概率失败: 未开启态势图计算")
            return 0.0, 0.0

    def _calculate_penetration_situational_map(self, threat: TargetProbabilityMap) -> float:
        """
        基于态势图的计算 - 最推荐的方法
        利用MultiTargetProbabilityMap的数据进行计算
        TO-DO: 1) 接入hn的路径规划模块  2) 维护白方预测的黑方对白方的态势图，并作为黑方可能突防路径的决策依据
        """
        if not self.blackboard:
            print("态势图突防概率计算失败: 黑板为空")
            return 0.5  # 默认概率

        try:
            # 贪婪估计：计算到出口的路径风险并选取最短路径 -- TODO: hn的路径规划部分
            exit_points = [Position(self.end_line_x, y) for y in
                           range(0, self.end_line_x, int(self.end_line_x / 10))]  # 假设左边界为出口
            threat_position = Position(threat.position[0], threat.position[1])
            min_exit_distance = min(threat_position.distance_to(exit) for exit in exit_points)
            closest_exit = min(exit_points, key=lambda exit: threat_position.distance_to(exit))

            # 采样路径上的威胁概率
            path_points = self._sample_path_points(threat_position, closest_exit, 5000)  # 5km采样间隔

            # 计算路径上的平均威胁概率
            penetration_prob = self.blackboard.get_prob_nearby_at(path_points, self.blackboard.enemy_situation_maps)

            return max(0.2, min(0.95, penetration_prob)), min_exit_distance

        except Exception as e:
            import traceback
            print(traceback.format_exc())
            print(f"态势图突防概率计算失败: {e}")
            return 0.5  # 默认概率

    def _calculate_detection_probability(self, position: Position, agents: Dict[str, Agent]) -> float:
        """计算指定位置的探测概率"""
        combined_prob = 0.0

        for agent in agents.values():
            if AgentAdapter.get_agent_state(agent) != AgentState.FREE:
                continue

            agent_pos = AgentAdapter.get_position(agent)
            distance = position.distance_to(agent_pos)

            # 根据智能体类型计算探测概率
            if isinstance(agent, Drone):
                # UAV扇形探测
                max_range = 60000  # 60km
                if distance <= max_range:
                    # 简化：不考虑角度，只考虑距离
                    agent_prob = max(0, 1 - distance / max_range)
                else:
                    agent_prob = 0
            else:
                # USV圆形探测
                max_range = 20000  # 20km
                if distance <= max_range:
                    agent_prob = max(0, 1 - distance / max_range)
                else:
                    agent_prob = 0

            # 合并探测概率（独立事件）
            combined_prob = combined_prob + agent_prob - combined_prob * agent_prob

        return min(1.0, combined_prob)

    def _sample_path_points(self, start: Position, end: Position, step_size: float) -> List[Position]:
        """在起点和终点之间采样路径点"""
        points = []
        distance = start.distance_to(end)
        num_steps = max(1, int(distance / step_size))

        for i in range(num_steps + 1):
            t = i / num_steps
            x = start.x + t * (end.x - start.x)
            y = start.y + t * (end.y - start.y)
            points.append(Position(x, y))

        return points

    def _calculate_path_risk(self, start: Position, end: Position, agents: Dict[str, Agent]) -> float:
        """计算路径风险（用于K-Min-Risk算法）"""
        path_points = self._sample_path_points(start, end, 100)  # 100m步长
        total_risk = 0.0

        for point in path_points:
            detection_prob = self._calculate_detection_probability(point, agents)
            undetected_prob = 1 - detection_prob

            # 转换为风险代价
            risk_cost = -math.log(max(undetected_prob, 1e-6))
            total_risk += risk_cost

        return total_risk

    def _calculate_heading_consistency(self, threat: TargetProbabilityMap) -> float:
        """
        计算航向一致性 Θ_heading = cos(angle(heading, direction_to_exit))
        """
        if not threat.velocity or (threat.velocity[0] == 0 and threat.velocity[1] == 0):
            return 0.0

        # 计算到最近出口的方向向量
        exit_point = Position(self.end_line_x, threat.position[1])  # 假设左边界为出口
        direction_to_exit = (
            exit_point.x - threat.position[0],
            exit_point.y - threat.position[1]
        )

        # 归一化方向向量
        exit_magnitude = (direction_to_exit[0] ** 2 + direction_to_exit[1] ** 2) ** 0.5
        if exit_magnitude == 0:
            return 0.0

        exit_unit = (direction_to_exit[0] / exit_magnitude, direction_to_exit[1] / exit_magnitude)

        # 归一化速度向量
        velocity_magnitude = (threat.velocity[0] ** 2 + threat.velocity[1] ** 2) ** 0.5
        if velocity_magnitude == 0:
            return 0.0

        velocity_unit = (threat.velocity[0] / velocity_magnitude, threat.velocity[1] / velocity_magnitude)

        # 计算余弦相似度
        cos_angle = velocity_unit[0] * exit_unit[0] + velocity_unit[1] * exit_unit[1]

        return cos_angle

    def set_weight_params(self, **kwargs):
        """设置威胁评分权重参数"""
        default_weights = {
            'alpha': 1.0,  # 突防概率权重
            'beta': 0.8,  # 距离衰减权重
            'gamma': 1.5,  # 航向一致性权重
            'delta': 0.3  # 速度权重
        }

        # 更新权重参数
        for key, value in kwargs.items():
            if key in default_weights:
                self.weight_params[key] = value

        print(f"更新权重参数: {self.weight_params}")

    def allocate_reckon_tasks_with_cbba(self, threats: Dict[int, TargetProbabilityMap],
                                        agents: List[Agent]) -> Dict[int, List[int]]:
        """使用CBBA算法分配任务"""
        # 生成各类任务
        explore_tasks = self.generate_threat_scan_tasks(threats, agents)

        # 使用CBBA求解
        allocation_result = self.cbba_integrator.solve_scan_task_allocation(agents, explore_tasks)

        # 更新活动任务
        for task in explore_tasks:
            self.active_tasks[task.task_id] = task

        return allocation_result

    def allocate_lock_tasks_with_cbba(self, threats: Dict[int, TargetProbabilityMap],
                                      agents: List[Agent]):
        """使用CBBA算法分配任务"""
        # 生成各类任务

        intercept_tasks = self.generate_intercept_tasks(threats, agents)

        # 使用CBBA求解
        # allocation_result, intercept_danger_tasks, intercept_task_pos = self.cbba_integrator.solve_lock_task_allocation(agents, intercept_tasks)
        allocation_result = self.cbba_integrator.solve_lock_task_allocation(agents, intercept_tasks)

        # 更新活动任务
        for task in intercept_tasks:
            self.active_tasks[task.task_id] = task

        # return allocation_result, intercept_tasks, intercept_danger_tasks, intercept_task_pos
        return allocation_result, intercept_tasks

    def handle_event_trigger(self, event_type: str, event_data: Dict) -> bool:
        """处理事件触发的重分配"""
        trigger_events = ['LOCK_FAIL', 'LOW_SOC', 'NEW_THREAT', 'FREEZE_END']

        if event_type in trigger_events:
            print(f"触发事件重分配: {event_type}")
            return True

        return False

    def execute_charge_update(self):
        """
        执行UAV充电任务更新
        """
        # 获取所有可用的白方UAV
        uav_available = []
        usv_available = []
        all_docked = True
        for uav in self.world.faction_agents['white']:
            if isinstance(uav, Drone):
                uav_available.append(uav)
                if uav.docking is None:
                    all_docked = False

        for usv in self.world.faction_agents['white']:
            if isinstance(usv, Boat):
                usv_available.append(usv)

        if not uav_available:
            print("取消充电任务分配：无无人机")
            return None
        elif not usv_available:
            print("取消充电任务分配：无可停靠船只")
            return None
        elif all_docked:
            print("取消充电任务分配：所有UAV已在充电")
            return None

        try:
            # charge_allocation_result = self.cbba_integrator.solve_charge_task_allocation_with_distributed_auction(
            #     uav_available, usv_available)
            charge_allocation_result = self.backup_charge_allocation(uav_available, usv_available)
            print("充电任务分配成功")
            return charge_allocation_result
        except Exception as e:
            import traceback
            print(traceback.format_exc())
            print(f"CBBA任务分配失败: {e}")
            return None

    def backup_charge_allocation(self, drones: List[Drone], boats: List[Boat]) -> Dict:
        """
        备用充电任务分配方法：将无人机分配给距离最近且电量最低的船只

        算法逻辑：
        1. 维护一个待分配船只列表
        2. 按照无人机的电量从低到高排序
        3. 依次为每架无人机分配距离最近的可用船只
        4. 被分配的船只从待分配列表中移除

        参数:
        drones: 无人机列表
        boats: 船只列表

        返回:
        Dict: 无人机ID到船只ID的分配映射
        """
        # 初始化分配结果
        final_allocation_result = {}

        # 如果没有可用资源，直接返回空分配
        if not drones or not boats:
            return final_allocation_result

        # 按电量从低到高排序无人机
        drones.sort(key=lambda d: d.power)

        # 维护一个待分配船只列表的副本
        remaining_boats = boats.copy()

        # 为每架无人机分配最近的船只
        for drone in drones:
            if not remaining_boats:
                # 没有剩余船只可分配
                drone.rtb_target = None
                continue

            # 找到距离当前无人机最近的船只
            closest_boat = min(remaining_boats, key=lambda boat: drone.distance_to(boat))

            # 分配该船只给无人机
            final_allocation_result[drone.id] = closest_boat.id
            drone.rtb_target = closest_boat

            # 从待分配列表中移除该船只
            remaining_boats.remove(closest_boat)

            print(f"无人机 {drone.id} 分配到船只 {closest_boat.id} 进行充电")

        return final_allocation_result

    def execute_reckon_update(self):
        """
        执行Reckon任务更新态势图
        """
        # 获取所有已被探测到的BSV对应的单目标图字典，并过滤掉belief小于等于0的目标
        all_threat_maps = {k: v for k, v in self.blackboard.situation_map.target_maps.items() if v.belief > 0}
        # 筛选熵值大于阈值或未检测时间过长的目标
        threats = {
            threat_map_id: threat_map for threat_map_id, threat_map in all_threat_maps.items()
            if threat_map.entropy > 0.35 or (threat_map.time - threat_map.last_detected) > 300 * (len(all_threat_maps))
        }


        if not threats:
            return

        # 获取所有可用的白方UAV
        uav_available = []
        for uav in self.world.faction_agents['white']:
            if isinstance(uav, Drone):
                # 有充足电力，未返航充电，且无锁定任务
                if (uav.state is AgentState.FREE or uav.state == AgentState.STANDBY or uav.state == AgentState.CHARGING) and \
                        uav.task_type != TaskType.RTB and uav.task_type != TaskType.ASSIT_LOCK:
                    for threat in threats.values():
                        # 仅选取航程可能满足任务需求的无人机
                        if uav.distance_to(threat.position) < (uav.power - 300) * uav.max_speed * 0.4:
                            uav_available.append(uav)
                            if uav.task_type == TaskType.RECKON:
                                uav.task_type = TaskType.IDLE
                                uav.task_list.clear()
                            break

        if not uav_available:
            print("没有可用的白方UAV进行态势图更新")
            return

        try:
            # 使用CBBA进行任务分配
            uav_allocation_result = self.allocate_reckon_tasks_with_cbba(threats, uav_available)
            print("Reckon(Update_Map)任务分配成功")
        except Exception as e:
            import traceback
            print(traceback.format_exc())
            print(f"CBBA任务分配失败: {e}")
            return

        # 更新UAV活动任务并执行任务
        for uav in uav_available:
            assigned_task_ids = uav_allocation_result[uav.id]

            if assigned_task_ids:
                uav.task_list = []
                for task_id in assigned_task_ids:
                    task = None
                    # 遍历威胁列表，找到任务目标
                    for t in threats.values():
                        if t.target_id == task_id:
                            task = t
                            # 添加Reckon任务信息至UAV
                            task_info = {
                                'task_id': task_id,
                                'task_type': 'RECKON',
                                'target_position': [task.position[0], task.position[1]],
                                'target_velocity': task.velocity,
                                'threat_score': 100,  # TODO:这里得到的是一个态势图而非EnhancedTask，没有威胁度评分属性，需修改
                                'status': 'assigned'
                            }
                            # 清空并更新UAV的任务列表
                            uav.task_list.append(task_info)
                            uav.task_type = TaskType.RECKON
                            print(
                                f"UAV {uav.id}: 分配Reckon任务 {task_id}，目标位置 ({task.position[0]:.0f}, {task.position[1]:.0f})")
                            break  # 任务已分配，跳出循环
                    if task is None:
                        print(f"未找到ID为{task_id}的目标，跳过此任务的分配")


            else:
                pass

    def execute_lock_operation(self, threats: Dict[int, TargetProbabilityMap], usv_agents: List[Boat]):
        """
        全局协调节点负责调动USV和UAV执行Locking任务:
        1. 根据当前态势图估计已侦测到的敌方BSV的威胁度,对我方可用USV进行Locking任务的分配
        2. 我方USV收到任务后前出执行,由全局协调节点负责检查我方USV距离任务坐标位置是否达到阈值距离D_0
        3. 若达到阈值距离,则全局协调者(GC)分配Scan_Lock任务给UAV(分配模块按频率freq + 动态事件触发(e.g.USV达到距离阈值;UAV返航补能; 新的UAV起飞; UAV坠落; USV被摧毁))
        4. 无人机抵达待侦察位置,具体策略由ZL负责;USV抵达拦截位置,等待符合探测要求，一旦符合立即跟踪目标并锁定
        5. 由全局协调节点对Locking任务分配模块时刻事件触发更新,触发事件为: USV被摧毁(同时调动UAV的Scan_Lock任务)
        """

        # # 获取所有已被探测到的BSV对应的单目标图字典
        # threats = self.blackboard.situation_map.target_maps

        # if not threats:
        #     return  # 没有威胁时直接返回

        # # 获取所有可用（未冻结且未被淘汰）的白方USV
        # usv_agents = self.world.get_all_white_boats()

        # if not usv_agents:
        #     print("没有可用的白方USV执行拦截任务")
        #     return

        # 使用CBBA求解拦截任务分配
        try:
            # usv_lock_allocation_result, intercept_tasks, usv_lock_danger_tasks, usv_task_pos = self.allocate_lock_tasks_with_cbba(threats, usv_agents)
            usv_lock_allocation_result, intercept_tasks = self.allocate_lock_tasks_with_cbba(threats, usv_agents)

            print("成功分配Lock任务")
        except Exception as e:
            import traceback
            print(traceback.format_exc())
            print(f"CBBA任务分配失败: {e}")
            return

        # 更新USV活动任务并执行任务
        for usv in usv_agents:
            assigned_task_ids = usv_lock_allocation_result.get(usv.id, [])

            # 清空并更新USV的任务列表
            usv.task_list = []

            # 为每个分配的任务创建任务项
            for task_id in assigned_task_ids:
                # 找到对应的任务对象
                task = None
                for t in intercept_tasks:
                    if t.target_id == task_id:
                        task = t
                        break

                if task is None:
                    continue

                # 如果存在危险任务评估
                # if task.target_id in usv_lock_danger_tasks[usv.id]:
                #     # 威胁评分过低（小于1.5）则放弃任务
                #     if task.threat_score / 3.0 <= 0.5:
                #         execute_task = False
                #         print(f"USV {usv.id}: 任务 {task_id} 威胁评分过低({danger_task.threat_score:.2f})，放弃执行")
                #         break

                # for cand_target_pos in usv_task_pos[usv.id]:
                #     if list(cand_target_pos.keys())[0] == task_id:
                #         target_pos = cand_target_pos[task_id]
                #         break

                # 添加任务到USV任务列表
                task_info = {
                    'task_id': task_id,
                    'task_type': 'LOCKING',
                    'target_position': [task.position.x, task.position.y],
                    'target_velocity': task.velocity,
                    'threat_score': task.threat_score,
                    'status': 'assigned'
                }
                usv.task_list.append(task_info)
                usv.task_type = TaskType.LOCK
                print(f"USV {usv.id}: 分配拦截任务 {task_id}，目标位置 ({task.position.x:.0f}, {task.position.y:.0f})")

        # 标记当前正在执行拦截操作
        self.status_locking_exe = True

    def calculate_distance_threshold(self, threat: TargetProbabilityMap, boat: Boat, drones: List[Drone]):
        # 计算阈值距离 D_0 = USV_MAX_Speed * mean_travel_time
        try:
            # 计算所有USV到该威胁的平均旅行时间
            travel_times = self._calculate_travel_time(threat, drones)
            if travel_times:
                mean_travel_time = sum(t for t in travel_times if t != float('inf')) / max(
                    len([t for t in travel_times if t != float('inf')]), 1)
                threshold_distance = boat.max_speed * mean_travel_time + AGENT_PARAMS[
                    'USV_LOCK_RANGE']  # 无人艇进入打击范围时令无人机抵达
            else:
                threshold_distance = 38000  # 默认(35+3)km阈值

        except Exception as e:
            print(f"计算阈值距离失败: {e}")
            threshold_distance = 38000  # 默认(35+3)km阈值
            return threshold_distance

        return threshold_distance
    
    def cluster_threats_in_K(self, k:int, threats: Dict[int, TargetProbabilityMap]):
        """
        使用 K-Means 对威胁进行聚类（KISS 版本）
        返回: {cluster_idx: [threat_map_id, ...]}
        - K 默认取: min(可用白方USV数量, 威胁数量)，至少为 1
        - 如 sklearn 不可用，回退为按 y 坐标排序轮转均分
        """
        if not threats:
            return {}

        # 收集坐标与键
        filtered_items = [(target_id, target) for target_id, target in threats.items() if target.belief > 0.0]
        ids = [target_id for target_id, t in filtered_items]
        X = np.array([[t.position[0], t.position[1]] for target_id, t in filtered_items], dtype=float)

        n_targets = len(ids)
        if n_targets == 0:
            return {}
        # 如果目标数量 <= k，则直接一目标一簇
        if n_targets <= k:
            return {i: [ids[i]] for i in range(n_targets)}
        # k 为 1 时全部归为一簇
        if k <= 1:
            return {0: ids}

        # K-Means / 回退策略
        try:
            labels = self.KMeans(X, n_clusters=k, n_init=5, random_state=42)
            clusters = {i: [] for i in range(k)}
            for idx, label in enumerate(labels):
                clusters[label].append(ids[idx])
            return clusters
        except Exception:
            # 回退：按 y 从小到大轮转分到 k 个簇
            import traceback
            print(traceback.format_exc())
            order = np.argsort(X[:, 1])
            clusters = {i: [] for i in range(k)}
            for rank, idx in enumerate(order):
                clusters[rank % k].append(ids[idx])
            return clusters
        
    def KMeans(self, X, n_clusters=8, n_init=10, random_state=None, max_iter=300, tol=1e-4, init="k-means++"):
        """实现与 sklearn 接近的 K-Means，并返回 labels。

        参数与行为对齐要点：
        - X: (n_samples, n_features)
        - n_init: 重复随机初始化的次数，选择 inertia 最小的一次
        - random_state: 控制随机性
        - max_iter, tol: 迭代收敛条件（基于中心位移）
        - init: 仅支持 "k-means++" 或 "random"

        返回：
        - labels: (n_samples,) 聚类标签，类型为 int
        """
        X = np.asarray(X, dtype=float)
        if X.ndim != 2:
            raise ValueError("X 必须是二维数组 (n_samples, n_features)")
        n_samples, n_features = X.shape
        if n_clusters <= 0 or n_clusters > n_samples:
            raise ValueError("n_clusters 必须在 1..n_samples 之间")

        if init not in ("k-means++", "random"):
            raise ValueError("init 仅支持 'k-means++' 或 'random'")

        rng_master = np.random.RandomState(random_state)

        def _init_centers_kpp(rng):
            centers = np.empty((n_clusters, n_features), dtype=float)
            # 选择第一个中心
            first_idx = rng.randint(0, n_samples)
            centers[0] = X[first_idx]
            # 后续中心按距离平方的概率采样
            closest_sq = np.full(n_samples, np.inf, dtype=float)
            for c in range(1, n_clusters):
                # 更新到最近已选中心的距离平方
                dist_sq = np.sum((X - centers[c - 1]) ** 2, axis=1)
                closest_sq = np.minimum(closest_sq, dist_sq)
                total = closest_sq.sum()
                if not np.isfinite(total) or total <= 0:
                    # 退化：随机补齐
                    idx = rng.randint(0, n_samples)
                else:
                    probs = closest_sq / total
                    # 累积分布抽样
                    cum = np.cumsum(probs)
                    r = rng.rand()
                    idx = int(np.searchsorted(cum, r, side="right"))
                    if idx >= n_samples:
                        idx = n_samples - 1
                centers[c] = X[idx]
            return centers

        def _init_centers_random(rng):
            indices = rng.choice(n_samples, size=n_clusters, replace=False)
            return X[indices].copy()

        def _e_step(centers):
            # 返回 (labels, inertia)
            # 计算到每个中心的距离平方并取最小
            # 为避免一次性分配大矩阵，逐中心累积最小值
            labels = np.empty(n_samples, dtype=int)
            inertia = 0.0
            # 矩阵化计算距离
            # dist_sq[i, k] = ||X[i]-centers[k]||^2
            # 直接一次性计算通常足够快且便于保持 sklearn 行为
            x_sq = np.sum(X ** 2, axis=1, keepdims=True)  # (n_samples, 1)
            c_sq = np.sum(centers ** 2, axis=1)          # (n_clusters,)
            # 使用 (x - c)^2 = x^2 + c^2 - 2 x·c
            distances = x_sq + c_sq[None, :] - 2.0 * X.dot(centers.T)
            labels = np.argmin(distances, axis=1)
            inertia = float(np.sum(distances[np.arange(n_samples), labels]))
            return labels, inertia

        def _m_step(labels):
            # 计算新中心；若出现空簇，则将该中心重置为当前最远样本
            centers = np.zeros((n_clusters, n_features), dtype=float)
            counts = np.bincount(labels, minlength=n_clusters).astype(int)
            for k in range(n_clusters):
                mask = (labels == k)
                if counts[k] > 0:
                    centers[k] = X[mask].mean(axis=0)
                else:
                    # 空簇：选择距离最近中心最远的样本作为新中心
                    # 先用临时中心避免依赖未定义中心
                    tmp_centers = centers.copy()
                    # 用已有非空中心填充，否则随机选择
                    non_empty = np.where(counts > 0)[0]
                    if len(non_empty) == 0:
                        tmp_centers[k] = X[rng_master.randint(0, n_samples)]
                    else:
                        tmp_centers = tmp_centers[non_empty]
                        x_sq = np.sum(X ** 2, axis=1, keepdims=True)
                        c_sq = np.sum(tmp_centers ** 2, axis=1)
                        distances = x_sq + c_sq[None, :] - 2.0 * X.dot(tmp_centers.T)
                        farthest_idx = int(np.argmax(np.min(distances, axis=1)))
                        centers[k] = X[farthest_idx]
            return centers

        best_inertia = float("inf")
        best_labels = None

        for _ in range(max(1, int(n_init))):
            rng = np.random.RandomState(rng_master.randint(0, 2**31 - 1))
            centers = _init_centers_kpp(rng) if init == "k-means++" else _init_centers_random(rng)

            for _ in range(max_iter):
                labels, inertia = _e_step(centers)
                new_centers = _m_step(labels)
                shift = float(np.sqrt(np.sum((centers - new_centers) ** 2)))
                centers = new_centers
                if shift <= tol:
                    break

            # 最后再计算一次 inertia（对应最终 centers）
            labels, inertia = _e_step(centers)
            if inertia < best_inertia:
                best_inertia = inertia
                best_labels = labels.copy()

        return best_labels
        
    def allocate_usv_to_clusters(self, clusters: Dict[int, List[int]]) -> Dict[int, List[Boat]]:
        """
        按聚类规模比例分配USV（KISS版）。
        输入: clusters = {cluster_id: [threat_id, ...]}
        返回: {cluster_id: [Boat, ...]}
        规则:
        - 先按每簇目标数占比给出整数配额（向下取整+按小数部分分配余量）
        - 当簇数 > USV 数时，按簇规模从大到小每簇分配1艘，直到用完
        - 具体USV采用“距离该簇质心最近”的贪婪匹配
        """

        if not clusters:
            return {}

        # 获取可用USV
        try:
            usv_agents: List[Boat] = [b for b in self.world.get_all_white_boats()]
        except Exception:
            usv_agents = []

        num_usv = len(usv_agents)
        if num_usv == 0:
            print(f"没有可用的usv")
            return {cid: [] for cid in clusters.keys()}

        cluster_ids = list(clusters.keys())
        sizes = np.array([len(clusters[cid]) for cid in cluster_ids], dtype=float)
        total_targets = float(np.sum(sizes))

        # 计算每簇的USV配额
        alloc_counts = {cid: 0 for cid in cluster_ids}

        raw = num_usv * sizes / total_targets
        floors = np.floor(raw).astype(int)
        frac = raw - floors
        for i, cid in enumerate(cluster_ids):
            alloc_counts[cid] = int(floors[i])

        remainder = num_usv - np.sum(floors)
        if remainder > 0:
            order = np.argsort(-frac)  # 余量按小数部分由大到小分配
            for idx in order[:remainder]:
                alloc_counts[cluster_ids[idx]] += 1

        # 保底：若簇有敌方BSV但被floor后为0，且总体上簇数不超过USV数，则至少分配1艘
        if len(cluster_ids) <= num_usv:
            size_by_cid = {cid: int(sizes[i]) for i, cid in enumerate(cluster_ids)}
            frac_by_cid = {cid: float(frac[i]) for i, cid in enumerate(cluster_ids)}
            added = 0
            for cid in cluster_ids:
                if size_by_cid.get(cid, 0) > 0 and alloc_counts.get(cid, 0) == 0:
                    alloc_counts[cid] = 1
                    added += 1
            if added > 0:
                donors = [cid for cid in cluster_ids if alloc_counts.get(cid, 0) > 1]
                donors = sorted(donors, key=lambda c: (-alloc_counts[c], frac_by_cid.get(c, 0.0)))
                to_remove = added
                for cid in donors:
                    while alloc_counts[cid] > 1 and to_remove > 0:
                        alloc_counts[cid] -= 1
                        to_remove -= 1
                    if to_remove == 0:
                        break

        # 若簇数多于USV数，改为给规模最大的前 num_usv 个簇各1艘
        if len(cluster_ids) > num_usv:
            print(f"簇数多于USV数，改为给规模最大的前 {num_usv} 个簇各1艘usv")
            order2 = np.argsort(-sizes)
            alloc_counts = {cid: 0 for cid in cluster_ids}
            for idx in order2[:num_usv]:
                alloc_counts[cluster_ids[idx]] = 1

        # 计算每簇质心
        threat_maps = self.world.faction_maps['white'].target_maps
        centers: Dict[int, np.ndarray] = {}
        for cid in cluster_ids:
            ids = clusters[cid]
            pts = []
            for tid in ids:
                if tid in threat_maps:
                    t = threat_maps[tid]
                    pts.append([t.position[0], t.position[1]])
            if pts:
                centers[cid] = np.mean(np.array(pts, dtype=float), axis=0)
            else:
                print(f"簇 {cid} 计算质心失败")
                centers[cid] = np.array([self.world.width / 2.0, self.world.height / 2.0], dtype=float)

        # 依据配额生成“簇槽位”列表（重复簇ID）
        slot_cluster_ids: List[int] = []
        for cid in cluster_ids:
            count = int(alloc_counts.get(cid, 0))
            if count > 0:
                slot_cluster_ids.extend([cid] * count)

        if not slot_cluster_ids:
            return {cid: [] for cid in cluster_ids}

        # 构造代价矩阵 H: 行=USV, 列=簇槽位，元素=USV到该簇质心的欧氏距离
        m = num_usv
        n = len(slot_cluster_ids)
        H = np.zeros((m, n), dtype=float)
        boat_pos = np.array([[b.x, b.y] for b in usv_agents], dtype=float)
        for j, cid in enumerate(slot_cluster_ids):
            c = centers[cid]
            diff = boat_pos - c.reshape(1, 2)
            H[:, j] = np.linalg.norm(diff, axis=1)

        # 分布式拍卖分配（最小化距离）
        try:
            # assignment, _ = self.cbba_integrator.distributed_auction_with_virtual_usv(H)
            raise Exception("测试贪婪分配")
        except Exception:
            import traceback
            # print("分布式拍卖失败，回退到就近贪婪分配\n" + traceback.format_exc())
            # 回退：每列取当前最小距离，避免重复
            assignment = np.full(n, 114514, dtype=int)
            used = set()
            for j in range(n):
                order = np.argsort(H[:, j])
                for r in order:
                    if r not in used:
                        assignment[j] = int(r + 1)
                        used.add(r)
                        break

        # 解析分配结果
        result: Dict[int, List[Boat]] = {cid: [] for cid in cluster_ids}
        for j, a in enumerate(assignment):
            cid = slot_cluster_ids[j]
            if isinstance(a, np.ndarray):
                a = int(a)
            if a == 114514:
                continue
            r = int(a) - 1
            if 0 <= r < m:
                b = usv_agents[r]
                if b not in result[cid]:
                    result[cid].append(b)

        return result

    def call_uav_assit_scan(self, agent: Boat, available_uavs: List[Drone]):
        """
        USV在合适条件时呼叫UAV协助探测
        """

        if not available_uavs:
            print(f"USV {agent.id}: 没有可用的UAV进行协助扫描")
            return False

        # 为USV的当前任务寻找最近的UAV
        if not agent.task_list:
            print(f"USV {agent.id}: 没有当前任务, 无需UAV协助")
            return False

        current_task = agent.task_list[0]
        target_position = Position(current_task['target_position'][0], current_task['target_position'][1])

        for drone in available_uavs:
            # 清空已有的协助锁定任务重分配，以匹配最优无人机
            if (drone.task_list and current_task["task_id"] == drone.task_list[0][
                "task_id"] and drone.task_type == TaskType.ASSIT_LOCK):
                drone.task_type = TaskType.IDLE
                drone.task_list = []

        # 选择最近的可用UAV
        best_uav = None
        min_distance = float('inf')

        for uav in available_uavs:
            uav_position = Position(uav.x, uav.y)
            distance = uav_position.distance_to(target_position)

            if distance < min_distance:
                min_distance = distance
                best_uav = uav

        if best_uav is None:
            print(f"USV {agent.id}: 未找到合适的UAV进行协助")
            return False

        # 将任务分配给UAV
        uav_task_info = {
            'task_id': current_task['task_id'],
            'task_type': TaskType.ASSIT_LOCK,
            'target_position': [target_position.x, target_position.y],
            'assist_usv_id': agent.id,
            'duration': 1800,
            'status': 'assigned'
        }

        print(
            f"UAV {best_uav.id}: 分配协助扫描任务 {current_task['task_id']}，目标位置 ({target_position.x:.0f}, {target_position.y:.0f})")

        # 清空UAV当前任务并分配新任务
        best_uav.task_list = [uav_task_info]
        best_uav.task_type = TaskType.ASSIT_LOCK
        return True
